#ifndef __COMM_MSG_DEV_INFO_H__
#define __COMM_MSG_DEV_INFO_H__

#include "comm/comm_topic_name.h"
#include "comm/comm_base.h"
#include <comm_msg/dev_info.h>
#include <ros/ros.h>
#include <iostream>
#include <list>
#include <vector>

using namespace std;

typedef struct {
    UINT08  chargerMotorState;
    UINT32  chargerMotorTravel;

    UINT08  liftMotorState;
    UINT08  rollMotorState;
    UINT16  leftLiftMotorTravel;
    UINT16  rightLiftMotorTravel;
    UINT16  leftRollMotorTravel;
    UINT16  rightRollMotorTravel;
    float   angleSensorOne;
    float   angleSensorTwo;

    BOOL    isStationEmergeStop;
    BOOL    isManualChargeMode;
    BOOL    proximitySw;
    BOOL    chargeSw;
    BOOL    waterSw;
    BOOL    isStationLockSw;
    BOOL    chargerTravelSw;
    BOOL    leftTravelSw;
    BOOL    rightTravelSw;
} DEV_INFO_STRU;

typedef enum {
    MOTOR_STATE_NORMAL            = 0x00,
    MOTOR_STATE_STICK_COMPLETE    = 0x01,
    MOTOR_STATE_WITHDRAW_COMPLETE = 0x02,
    MOTOR_STATE_STICKING          = 0x03,
    MOTOR_STATE_WITHDRAWING       = 0x04,
    MOTOR_STATE_ABNORMAL          = 0xFF,
} MOTOR_STATE_ENUM;

/**************************************************************************************
功能描述: 发布消息
修改记录:
**************************************************************************************/
class DEV_INFO_SENDER_C
{
public:
    DEV_INFO_SENDER_C(ros::NodeHandle &nh)
    {
        publisher_ = nh.advertise<comm_msg::dev_info>(TOPIC_NAME_DEV_INFO, 1);

        InitMsg();
    }

    void Send(const DEV_INFO_STRU &stDevInfo)
    {
        stMsg_.chargerMotorState    = stDevInfo.chargerMotorState;
        stMsg_.chargerMotorTravel   = stDevInfo.chargerMotorTravel;
        stMsg_.liftMotorState       = stDevInfo.liftMotorState;
        stMsg_.rollMotorState       = stDevInfo.rollMotorState;
        stMsg_.leftLiftMotorTravel  = stDevInfo.leftLiftMotorTravel;
        stMsg_.rightLiftMotorTravel = stDevInfo.rightLiftMotorTravel;
        stMsg_.leftRollMotorTravel  = stDevInfo.leftRollMotorTravel;
        stMsg_.rightRollMotorTravel = stDevInfo.rightRollMotorTravel;
        stMsg_.angleSensorOne       = stDevInfo.angleSensorOne;
        stMsg_.angleSensorTwo       = stDevInfo.angleSensorTwo;
        stMsg_.isStationEmergeStop  = stDevInfo.isStationEmergeStop;
        stMsg_.isManualChargeMode   = stDevInfo.isManualChargeMode;
        stMsg_.proximitySw          = stDevInfo.proximitySw;
        stMsg_.chargeSw             = stDevInfo.chargeSw;
        stMsg_.waterSw              = stDevInfo.waterSw;
        stMsg_.stationLockSw        = stDevInfo.isStationLockSw;
        stMsg_.chargerTravelSw      = stDevInfo.chargerTravelSw;
        stMsg_.leftTravelSw         = stDevInfo.leftTravelSw;
        stMsg_.rightTravelSw        = stDevInfo.rightTravelSw;

        publisher_.publish(stMsg_);
    }
    
private:
    ros::Publisher     publisher_;
    comm_msg::dev_info stMsg_;

    void InitMsg(void)
    {
        stMsg_.chargerMotorState     = 0;
        stMsg_.chargerMotorTravel    = 0;
        stMsg_.liftMotorState        = 0;
        stMsg_.rollMotorState        = 0;
        stMsg_.leftLiftMotorTravel   = 0;
        stMsg_.rightLiftMotorTravel  = 0;
        stMsg_.leftRollMotorTravel   = 0;
        stMsg_.rightRollMotorTravel  = 0;
        stMsg_.angleSensorOne        = 0.00;
        stMsg_.angleSensorTwo        = 0.00;
        stMsg_.isStationEmergeStop   = false;
        stMsg_.isManualChargeMode    = false;
        stMsg_.proximitySw           = false;
        stMsg_.chargeSw              = false;
        stMsg_.waterSw               = false;
        stMsg_.stationLockSw         = false;
        stMsg_.chargerTravelSw       = false;
        stMsg_.leftTravelSw          = false;
        stMsg_.rightTravelSw         = false;
    }
};

/**************************************************************************************
功能描述: 接收消息
修改记录:
**************************************************************************************/
class DEV_INFO_RECEIVER_C
{
public:
    DEV_INFO_RECEIVER_C(ros::NodeHandle &nh, double overTime = 0.5)
    {
        objInvalidTimer_.SetOverTime(overTime);
        receiver_ = nh.subscribe(TOPIC_NAME_DEV_INFO, 1, &DEV_INFO_RECEIVER_C::ReceiveMsgCallBack, this);
    }

    const DEV_INFO_STRU *Get(void) const
    {
        if (objInvalidTimer_.IsTimeOut()) {
            return nullptr;
        }

        return &stDevInfo;
    }

private:
    ros::Subscriber  receiver_;

    TIMER_C          objInvalidTimer_;       /* 数据失效定时器，超时后数据无效 */
    DEV_INFO_STRU    stDevInfo;

    void ReceiveMsgCallBack(const comm_msg::dev_info::ConstPtr &pstMsg)
    {
        objInvalidTimer_.Reset();

        stDevInfo.chargerMotorState    = pstMsg->chargerMotorState;
        stDevInfo.chargerMotorTravel   = pstMsg->chargerMotorTravel;
        stDevInfo.liftMotorState       = pstMsg->liftMotorState;
        stDevInfo.rollMotorState       = pstMsg->rollMotorState;
        stDevInfo.leftLiftMotorTravel  = pstMsg->leftLiftMotorTravel;
        stDevInfo.rightLiftMotorTravel = pstMsg->rightLiftMotorTravel;
        stDevInfo.leftRollMotorTravel  = pstMsg->leftRollMotorTravel;
        stDevInfo.rightRollMotorTravel = pstMsg->rightRollMotorTravel;
        stDevInfo.angleSensorOne       = pstMsg->angleSensorOne;
        stDevInfo.angleSensorTwo       = pstMsg->angleSensorTwo;
        stDevInfo.isStationEmergeStop  = pstMsg->isStationEmergeStop;
        stDevInfo.isManualChargeMode   = pstMsg->isManualChargeMode;
        stDevInfo.proximitySw          = pstMsg->proximitySw;
        stDevInfo.chargeSw             = pstMsg->chargeSw;
        stDevInfo.waterSw              = pstMsg->waterSw;
        stDevInfo.isStationLockSw      = pstMsg->stationLockSw;
        stDevInfo.chargerTravelSw      = pstMsg->chargerTravelSw;
        stDevInfo.leftTravelSw         = pstMsg->leftTravelSw;
        stDevInfo.rightTravelSw        = pstMsg->rightTravelSw;
    }
};

#endif

